#include <cmath>
#include "conio.h"
#include "cv.h"
#include "highgui.h"
#include "stdio.h"
#include "stdlib.h"
#include "windows.h"
#include <tchar.h>
#include "Serial.h"


using namespace std;

#define SCREEN_HEIGHT 25

IplImage* frame;
IplImage* frame2;
IplImage *colimgbot;
IplImage *colimgbot2;
IplImage *monoimgbot;
IplImage *monoimgbot2;
IplImage *resimgbot;
IplImage *ref;
uchar *refData;
uchar *refData2;
uchar *refDataRes;

int stepH = 1, stepS = 5, stepV = 5;
int timeOut = 1;

int i,j,k;
int temp=0;
int heighthsv,widthhsv,stephsv,channelshsv;
int heighthsv2,widthhsv2,stephsv2,channelshsv2;
int heightmono,widthmono,stepmono,channelsmono;
uchar *datahsv,*datamono, *datahsv2, *datamonoRes;
int sthreshold=140;
double hlower=175,hupper=5;

double toleranceH = 14;
double toleranceS = 25;
double toleranceV = 75;

CSerial serial;
LONG    lLastError = ERROR_SUCCESS;

int currentR, currentG, currentB;

CvMemStorage* storage;
CvSeq* contour = 0;

int mode;

void printTolerances()
{
	system ( "cls" );
	printf("toleranceH+   RIGHT\n");
	printf("toleranceH-   LEFT\n");
	printf("toleranceS+   UP\n");
	printf("toleranceS-   DOWN\n");
	printf("toleranceV+   PAGE UP\n");
	printf("toleranceV-   PAGE DOWN\n\n\n");

	printf("H\n");
	printf("toleranceH: %.2f\n\n",toleranceH);

	printf("S\n");
	printf("toleranceS: %.2f\n\n",toleranceS);

	printf("V\n");
	printf("toleranceV: %.2f\n\n",toleranceV);


	printf("Current R: %d\n\n",currentR);

	printf("Current G: %d\n\n",currentG);

	printf("Current B: %d\n\n",currentB);
}

uchar* doStuff(uchar* datahsv, uchar* output)
{
	for(int i=0;i<(heighthsv);i++)
	{
		for(int j=0;j<(widthhsv);j++)
		{

			// HSV
			int indexH = (i)*stephsv+j*channelshsv;
			int indexS = (i)*stephsv+j*channelshsv + 1;
			int indexV = (i)*stephsv+j*channelshsv + 2;
			// HLS
			//int indexH = (i)*stephsv+j*channelshsv;
			//int indexS = (i)*stephsv+j*channelshsv + 2;

			int currentH = datahsv[indexH];
			int currentS = datahsv[indexS];
			int currentV = datahsv[indexV];

			double refHUp = refData[0]	+	toleranceH;
			double refHDown = refData[0];//	-	toleranceH;

			// HSV
			double refSUp = refData[1]	+	toleranceS;
			double refSDown = refData[1]	-	toleranceS;
			double refVUp = refData[2]	+	toleranceV;
			double refVDown = refData[2]	-	toleranceV;
			// HLS
			//double refSUp = refData[2]	+	toleranceS;
			//double refSDown = refData[2]	-	toleranceS;

			if	(	(	(	currentH	<	refHUp	) && (	currentH	>	refHDown	)	) &&
					(	(	currentS	<	refSUp	) && (	currentS	>	refSDown	)	) &&
					(	(	currentV	<	refVUp	) && (	currentV	>	refVDown	)	)	)
					output[i*stepmono+j*channelsmono]=255;
			else
					output[i*stepmono+j*channelsmono]=0;
		}
	}

	//for(int i=0;i<(heighthsv);i++)
	//	for(int j=0;j<(widthhsv);j++)
	//		if(!(datamono[i*stepmono+j*channelsmono]==0 || datamono[i*stepmono+j*channelsmono]==255))
	//			datamono[i*stepmono+j*channelsmono]=0;

	return output;
}


void doLogicalANDOperation(uchar* mono, uchar* mono2)
{
	for(int i=0;i<(heighthsv);i++)
	{
		for(int j=0;j<(widthhsv);j++)
		{
			int indexMono = (i)*stepmono+j*channelsmono;
			if(mono[indexMono] == 255 && mono2[indexMono]==255)
				datamono[indexMono] = 255;
			else
				datamono[indexMono] = 0;
			//datamono[indexMono] = mono[indexMono];
		}		
	}
}

void setRefValsRGB(int R, int G, int B)
{
	currentR = R;
	currentG = G;
	currentB = B;

	ref = cvCreateImage(cvSize(100, 100), IPL_DEPTH_8U, 3);
			
	refData = (uchar *)ref->imageData;
	
	for (int p = 0 ; p < (ref->width) ; p++)
	{
		for(int q = 0 ; q < (ref->height) ; q++)
		{
			// color model BGR
			refData[p*(ref->widthStep)+q*(ref->nChannels)] = B;
			refData[p*(ref->widthStep)+q*(ref->nChannels)+1] = G;
			refData[p*(ref->widthStep)+q*(ref->nChannels)+2] = R;
		}
	}

	//cvShowImage("ref - ORIGINAL",ref);

	cvCvtColor(ref,ref,CV_RGB2HSV);

	refData = (uchar *)ref->imageData;
	
	//cvShowImage("ref - HLS",ref);

	printTolerances();
}

void setRefValsHSV(int H, int S, int V)
{
	ref = cvCreateImage(cvSize(100, 100), IPL_DEPTH_8U, 3);
			
	refData = (uchar *)ref->imageData;
	
	for (int p = 0 ; p < (ref->width) ; p++)
	{
		for(int q = 0 ; q < (ref->height) ; q++)
		{
			refData[p*(ref->widthStep)+q*(ref->nChannels)] = H;
			refData[p*(ref->widthStep)+q*(ref->nChannels)+1] = S;
			refData[p*(ref->widthStep)+q*(ref->nChannels)+2] = V;
		}
	}

	cvCvtColor(ref,ref,CV_HSV2RGB);
	
	//cvShowImage("ref - ORIGINAL",ref);

	cvCvtColor(ref,ref,CV_RGB2HSV);

	refData = (uchar *)ref->imageData;
	
	//cvShowImage("ref - HLS",ref);

	printTolerances();
}

void on_mouse( int event, int x, int y, int flags, void* param )
{
	switch( event )
    {
		case CV_EVENT_LBUTTONDOWN:
        {
			uchar *tmpData = (uchar *)frame->imageData;
			// color model BGR
			setRefValsRGB(tmpData[((y-1)*(frame->width*3)) + x*3 + 2],tmpData[((y-1)*(frame->width*3)) + x*3 + 1],
				tmpData[((y-1)*(frame->width*3)) + x*3]);
		}
	 }
}


void addConturs()
{
	cvFindContours(monoimgbot, storage, &contour, sizeof(CvContour), mode, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));//draw the contourcvDrawContours(contourImg, contour, CV_RGB(0, 255, 0), CV_RGB(255, 0, 0), 2, 2, 8);
	cvDrawContours(frame, contour, CV_RGB(0, 255, 0), CV_RGB(255, 0, 0), 2, 1, 8);
	//cvDrawContours(monoimgbot, contour,, 2, 2, 8);	
}

int ShowError (LONG lError, LPCTSTR lptszMessage)
{
	// Generate a message text
	TCHAR tszMessage[256];
	wsprintf(tszMessage,_T("%s\n(error code %d)"), lptszMessage, lError);

	// Display message-box and return with an error-code
	::MessageBox(0,tszMessage,_T("Hello world"), MB_ICONSTOP|MB_OK);
	return 1;
}

void kretanje(char motor, int brzina)
{
	if(motor == 'l' || motor=='L')
	{
		if(brzina<0)
		{
			brzina=brzina*(-1);
			brzina = brzina + 128;
		}
		else
			brzina = brzina + 192;
	}
	else
	{
		if(motor == 'd' || motor == 'D')
		{
			if(brzina<0)
			{
				brzina=brzina*(-1);
				brzina = brzina + 64;
			}
		}
	}
	
	//char* aa = (char*)&brzina;	
    //lLastError = serial.Write(aa);
	//if (lLastError != ERROR_SUCCESS)
	//	return ::ShowError(serial.GetLastError(), _T("Unable to send data"));
	printf("\nMotor: %c\n",motor);
	printf("\nBrzina: %d\n\n",brzina);
}

void stopEngines()
{
	kretanje('l',1);
	kretanje('d',1);
}

int main()
{
	i=j=k=0;

	printTolerances();
	
	//cvNamedWindow("ref - ORIGINAL", CV_WINDOW_AUTOSIZE);
	//cvNamedWindow("ref - HLS", CV_WINDOW_AUTOSIZE);	
	//cvNamedWindow("original - HLS", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("original", CV_WINDOW_AUTOSIZE);
	cvNamedWindow("SEARCH",CV_WINDOW_AUTOSIZE);

	storage =  cvCreateMemStorage(0);

	mode = CV_RETR_EXTERNAL;
	//mode = CV_RETR_CCOMP; 

	CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
	if( !capture )
	{
		fprintf( stderr, "ERROR: capture is NULL \n" );
		getchar();
		exit(0);
	}
	frame = cvQueryFrame( capture );
	frame2 = cvQueryFrame( capture );

	refData = (uchar *)frame->imageData;
	refData2 = (uchar *)frame2->imageData;

	colimgbot = cvCreateImage( cvGetSize(frame), 8, 3 );
	colimgbot2 = cvCreateImage( cvGetSize(frame2), 8, 3 );

	monoimgbot = cvCreateImage( cvGetSize(frame), 8, 1 );
	monoimgbot2 = cvCreateImage( cvGetSize(frame2), 8, 1 );

	setRefValsRGB(190,0,0);




	// Attempt to open the serial port (COM1)
    //lLastError = serial.Open(_T("COM1"),0,0,false);
	//if (lLastError != ERROR_SUCCESS)
	//	return ::ShowError(serial.GetLastError(), _T("Unable to open COM-port"));

    // Setup the serial port (9600,N81) using hardware handshaking
    //lLastError = serial.Setup(CSerial::EBaud2400,CSerial::EData8,CSerial::EParNone,CSerial::EStop1);
	//if (lLastError != ERROR_SUCCESS)
	//	return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port setting"));

	// Setup handshaking
	//lLastError = serial.SetupHandshaking(CSerial::EHandshakeOff);
	//if (lLastError != ERROR_SUCCESS)
	//	return ::ShowError(serial.GetLastError(), _T("Unable to set COM-port handshaking"));

	//kretanje('l',12);
	//kretanje('d',12);

	while( 1 ) 
	{
		
		frame = cvQueryFrame( capture );
		frame2 = cvQueryFrame( capture );
		

		if(frame==NULL || frame2==NULL)
		{
			puts("unable to load the frame");
			exit(0);
		}	

		//------------------------------------------------------------------

		heighthsv = colimgbot->height;

		widthhsv = colimgbot->width;

		stephsv = colimgbot->widthStep;

		channelshsv = colimgbot->nChannels;

		datahsv = (uchar *)colimgbot->imageData;

		//--------------------------

		heighthsv2 = colimgbot2->height;

		widthhsv2 = colimgbot2->width;

		stephsv2 = colimgbot2->widthStep;

		channelshsv2 = colimgbot2->nChannels;

		datahsv2 = (uchar *)colimgbot2->imageData;

		//--------------------------

		heightmono = monoimgbot->height;

		widthmono = monoimgbot->width;

		stepmono = monoimgbot->widthStep;

		channelsmono = monoimgbot->nChannels;

		datamono = (uchar *)monoimgbot->imageData;

		//cvSmooth(frame,frame, CV_GAUSSIAN,9,0,0,0);
		//cvSmooth(frame2,frame2, CV_GAUSSIAN,9,0,0,0);

		//--------------------------

		cvCvtColor(frame,colimgbot,CV_RGB2HSV);
		cvCvtColor(frame2,colimgbot2,CV_RGB2HSV);

		

		//colimgbot = frame;
		//colimgbot2 = frame2;

		uchar* output1 = new uchar[heighthsv*widthhsv*3];
		uchar* output2 = new uchar[heighthsv*widthhsv*3];

		uchar* mono = doStuff(datahsv,output1);
		uchar* mono2 = doStuff(datahsv2,output2);

		doLogicalANDOperation(mono,mono2);

		delete output1;
		delete output2;

		int x_top, y_top, x_bottom, y_bottom;
		int x_wanted;

		double alfa, lng;

		int pix_no;

		int tol_x = 10;		

		x_wanted = widthhsv/2;

		for(int i=0;i<(heighthsv);i++)
		{
			for(int j=0;j<(widthhsv);j++)
			{
				int indexMono = (i)*stepmono+j*channelsmono;
				if(datamono[indexMono]==255)
				{
					x_top = j;
					y_top = i;


					if(x_top > (x_wanted - tol_x) && x_top < (x_wanted + tol_x))
					{
						printf("\nSTOP!\n");
						kretanje('d',12);
						kretanje('l',12);
					}
					
					if(x_top < (x_wanted - tol_x))
					{
						printf("\nTERAJ LEVO!\n");
						// The serial port is now ready and we can send/receive data. If
						// the following call blocks, then the other side doesn't support
						// hardware handshaking.
						kretanje('l',1);
						kretanje('d',12);
					}

					if(x_top > (x_wanted + tol_x))
					{
						printf("\nTERAJ DESNO!\n");
						// The serial port is now ready and we can send/receive data. If
						// the following call blocks, then the other side doesn't support
						// hardware handshaking.
						kretanje('l',12);
						kretanje('d',1);												
					}

					pix_no = heighthsv - y_top;

					alfa = pix_no * 0.0625;
					lng = tan((alfa + 41.52)*3.14159/180) * 30.5;
					printf("\nUdaljenost: %lf", lng);
					break;
				}
			}
		}

		//cvLine(frame, cvPoint(0,heighthsv/4), cvPoint(widthhsv,heighthsv/4), cvScalar(255,0,0), 1);

		//cvLine(frame, cvPoint(0,heighthsv/2), cvPoint(widthhsv,heighthsv/2), cvScalar(255,0,0), 1);

		//cvLine(frame, cvPoint(0,heighthsv/2+heighthsv/4), cvPoint(widthhsv,heighthsv/2+heighthsv/4), cvScalar(255,0,0), 1);

		



		//cvRectangle(monoimgbot, cvPoint(100,100), cvPoint(200,150), cvScalar(255,0,0), 1);
		

		//tmpData[((y-1)*(frame->width*3)) + x*3]

		//addConturs();

		//cvRectangle(monoimgbot, cvPoint(100,100), cvPoint(200,150), cvScalar(255,0,0), 1);

		//CvFont font;

		//double hScale=1.0;
		//double vScale=1.0;
		//int    lineWidth=1;
		//cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, hScale,vScale,0,lineWidth);

		//cvPutText (frame,"Test test",cvPoint(50,100), &font, cvScalar(0,0,0));

		//cvErode(monoimgbot,monoimgbot,0,2);
		cvDilate( monoimgbot,monoimgbot,0,5);

		// New reF color by HSV
		if( (cvWaitKey(timeOut)) == 104 )			
		{
			int H, S, V;

			printf("\nH: ");
			scanf("%d",&H);
			printf("\nS: ");
			scanf("%d",&S);
			printf("\nV: ");
			scanf("%d",&V);

			setRefValsHSV(H,S,V);

			printf("\nOK!\n");
		}

		// New ref color by RGB
		if( (cvWaitKey(timeOut)) == 114 )			
		{
			int R, G, B;

			printf("\nR: ");
			scanf("%d",&R);
			printf("\nG: ");
			scanf("%d",&G);
			printf("\nB: ");
			scanf("%d",&B);

			setRefValsRGB(R,G,B);

			printf("\nOK!\n");
		}

		
		// Arrow RIGHT - toleranceH+
		if( (cvWaitKey(timeOut)) == 2555904 )			
		{
			toleranceH = toleranceH + stepH;
			printTolerances();
		}	

		// Arrow LEFT - toleranceH-
		if( (cvWaitKey(timeOut)) == 2424832 )	
		{
			toleranceH = toleranceH - stepH;
			printTolerances();
		}	
			
		// Arrow UP - toleranceS+
		if( (cvWaitKey(timeOut)) == 2490368 )	
		{
			toleranceS = toleranceS + stepS;
			printTolerances();
		}			

		// Arrow DOWN - toleranceS-
		if( (cvWaitKey(timeOut)) == 2621440 )	
		{
			toleranceS = toleranceS - stepS;
			printTolerances();
		}	

		// Page UP - toleranceV+
		if( (cvWaitKey(timeOut)) == 2162688 )	
		{
			toleranceV = toleranceV + stepV;
			printTolerances();
		}	

		// Page DOWN - toleranceV-
		if( (cvWaitKey(timeOut)) == 2228224 )	
		{
			toleranceV = toleranceV - stepV;
			printTolerances();
		}	

		if( (cvWaitKey(timeOut)) == 27 )	
			break;
		

		cvShowImage("original",frame);
		//cvShowImage("original - HLS",colimgbot);
		cvShowImage("SEARCH",monoimgbot);

		cvSetMouseCallback("original",on_mouse);

		Sleep(66);
	}

	//cvDestroyWindow("original - HLS");
	cvDestroyWindow("original");
	cvDestroyWindow("SEARCH");
	//cvDestroyWindow("ref - HLS");
	//cvDestroyWindow("ref - ORIGINAL");

	// Close the port again
    serial.Close();
	
	return 0;
}